from project_util import *
import sys
import heapq
import collections
import profile

start = None
goal = None
counter = None
Hmin = {}
Hmax = {}
paths = {}
OPEN = None
CLOSED = None


def fhmin(x):
    try:
        return Hmin[x]
    except KeyError:
        return -1


def fhmax(x):
    try:
        return Hmax[x]
    except KeyError:
        return -1


class S(object):
    def __init__(self, coord):
        self.coord = coord
        self.generated = 0
        self.idd = 0
        self.reusable_tree = None
        self.g = None
        self.h = None

    def __eq__(self, other):
        if other is None or other.coord is None:
            return False
        return self.coord == other.coord

    def __ne__(self, other):
        return not self.__eq__(other)

    def __cmp__(self, other):
        return (self.g + self.h).__cmp__(other.g + other.h)

    def __hash__(self):
        return self.coord.__hash__()

    def __str__(self):
        return self.coord.__str__()

    def __repr__(self):
        return self.__str__()


def succ(s):
    neigh = [S(addt(s, (1, 0))), S(addt(s, (0, 1))), S(addt(s, (-1, 0))), S(addt(s, (0, -1)))]
    return [s for s in neigh if not soccupied(start.coord, s.coord)]


def H(s):
    return abs(s.coord[0] - goal.coord[0]) + abs(s.coord[1] - goal.coord[1])


def initialize_state(s):
    if s.generated == 0:
        s.g = sys.maxint
        s.h = H(s)
    elif s.generated != counter:
        s.g = sys.maxint
    s.generated = counter


def add_path(s):
    global paths, Hmax, Hmin
    if s != goal:
        paths[s.idd].append(counter)
    Hmin[counter] = s.h
    Hmax[counter] = start.h
    paths[counter] = list()
    while s != start:
        saux = s
        s = s.searchtree
        s.idd = counter
        s.reusable_tree = saux


def remove_path(s):
    global Hmax, paths
    x = s.idd
    if fhmax(x) > s.reusable_tree.h:
        Hmax[x] = s.reusable_tree.h
    QUEUE = collections.deque([])
    for xp in paths[x]:
        if fhmax(x) < fhmin(xp):
            QUEUE.append(xp)
            paths[x].remove(xp)
    while len(QUEUE) > 0:
        x = QUEUE.popleft()
        if fhmax(x) > fhmin(x):
            Hmax[x] = fhmin(x)
            for xp in paths[x]:
                QUEUE.append(xp)
                paths[x].remove(xp)


def compute_path():
    while len(OPEN) > 0:
        s = heapq.heappop(OPEN)
        if s == goal or s.h <= fhmax(s.idd):
            # s is in the reusable tree
            for sp in CLOSED:
                sp.h = s.g + s.h - sp.g
            add_path(s)
            return True
        CLOSED.append(s)
        for sp in succ(s.coord):
            initialize_state(sp)
            if sp.g > (s.g + 1):
                sp.g = s.g + 1
                sp.searchtree = s
                if sp in OPEN:
                    OPEN.remove(sp)
                heapq.heappush(OPEN, sp)
    return False


def to_path(g):
    append = list.append
    path = list()
    while g.reusable_tree is not None:
        append(path, g.reusable_tree)
        g = g.reusable_tree
    return path


def treeaa(A, B):
    global start, goal, counter, Hmax, OPEN, CLOSED
    start = S(A)
    goal = S(B)
    counter = 1
    Hmax[0] = -1
    while start != goal:
        initialize_state(start)
        start.g = 0
        OPEN = list()
        CLOSED = list()
        heapq.heappush(OPEN, start)
        if not compute_path():
            raise Exception('FAILURE!')  # Failure: the goal state is unreachable
        while start.h <= fhmax(start.idd):
            start = start.reusable_tree
            yield start.coord
            if start.reusable_tree is not None and occupied(start.reusable_tree.coord):
                remove_path(start.reusable_tree)
        counter += 1
    print 'SUCCESS!'



if __name__ == '__main__':
    profile.run('treeaa((1,1), (11,11))')